#include "String.h"
#include "PS2Protocol.h"
#include "PS2Keyboard.h"

#define PS2K_COMMAND_QUEUE_SIZE 20  // Can reduce if not running test with loads of commands.

typedef enum {
    PS2K_NOP = 0x00,
    PS2K_RESET = 0xFF,
    PS2K_ECHO = 0xEE,
    PS2K_LED = 0xED,
    PS2K_DEFAULTS = 0xF6,
    PS2K_ENABLE_SCAN = 0xF4,
    PS2K_DISABLE_SCAN = 0xF5,
    PS2K_SCAN_CODE_SET = 0xF0,
    PS2K_IDENTIFY = 0xF2,
    PS2K_TYPE_RATE = 0xF3,

    // Responses
    PS2K_ACK = 0xFA,
    PS2K_RESEND = 0xFE,
    PS2K_FAIL1 = 0xFC,
    PS2K_FAIL2 = 0xFD
} PS2K_COMMANDS;

static PS2Keyboard_Callback_Function keyboardCallback = NULL;
// Commands will go in a queue.  Shouldn't need a big queue since
// commands are only sent at startup and then to set the LED's.
// If many commands are being sent at a time then the queue size may need
// to be made bigger.
static int commandQueue[PS2K_COMMAND_QUEUE_SIZE + 1][2];
static int head = 0;   // Always points to empty buffer.
static int tail = 0;   // Points to last item in buffer.
static PS2K_COMMANDS pendingCommand = PS2K_NOP;
static int pendingData = -1;
static bool receiveInProgress = false;
static unsigned char scanCodes[3];
static unsigned char scanCodeCount = 0;

static unsigned char multiScanCode[] = {0xE0};
static unsigned char RELEASE_CODE = 0xF0;


void PS2K_AddToQueue(PS2K_COMMANDS command, int data){
    if (head + 1 == tail) return;
    if (head + 1 == PS2K_COMMAND_QUEUE_SIZE + 1 && tail == 0) return;
    commandQueue[head][0] = (int)command;
    commandQueue[head][1] = data;
    head++;
    if (head == PS2K_COMMAND_QUEUE_SIZE + 1) head = 0;
    commandQueue[head][0] = PS2K_NOP;   // Clear data to make it easier to debug
    commandQueue[head][1] = 0;
}

void PS2K_GetFromQueue(PS2K_COMMANDS* command, int* data){
    if (tail == head){
        *command = 0;
        *data = 0;
        return;
    }
    *command = commandQueue[tail][0];
    *data = commandQueue[tail][1];
    commandQueue[tail][0] = PS2K_NOP;   // Clear data to make it easier to debug
    commandQueue[tail][1] = 0;   // Clear data to make it easier to debug
    tail++;
    if (tail == PS2K_COMMAND_QUEUE_SIZE + 1) tail = 0;
}

void PS2K_ProcessQueue(){
    PS2K_COMMANDS command;
    int data;
    
    if (!pendingCommand && receiveInProgress == false){
        PS2K_GetFromQueue(&command, &data);
        if (command != PS2K_NOP){
            pendingCommand = command;
            pendingData = data;
            PS2_Send(command);  // This uses interrupts so this will complete before its done.
        }
    }
}

// Call back from PS2 protocal with scan code or status
void PS2P_Available_Int(){
    unsigned char scanCode;
    unsigned char size;
    unsigned char loop;
    bool more = false;

    if (pendingCommand == PS2K_NOP){    // This is a key press or release
        receiveInProgress = true;
        scanCode = PS2_GetScanCode();
        // Check if this is a release code or multi code
        if (scanCode == RELEASE_CODE){
            more = true;
        } else {
            size = sizeof(multiScanCode) / sizeof(unsigned char);
            for (loop = 0; loop < size; loop++){
                if (scanCode == multiScanCode[loop]){
                    more = true;
                    break;
                }
            }
        }
        if (more == false){
            receiveInProgress = false;
            scanCodes[scanCodeCount] = scanCode;
            if (keyboardCallback != NULL){
                keyboardCallback(scanCodes[0],scanCodes[1],scanCodes[2]);
            }
            scanCodeCount = 3;  // Used to force clear
        } else {
            scanCodes[scanCodeCount] = scanCode;
            scanCodeCount++;
        }
    } else {    // If command was pending then this is the result for the command

        if (keyboardCallback != NULL){
            // There is a special case for Get scan code set.  It is the only
            // command that has data and receives 2 data bytes.  For this case
            // we will ignore the data of 0 and just use the 2 return bytes.
            // There is a special case for Reset.  It has 2 return bytes.
            if((pendingCommand == PS2K_RESET || (pendingCommand == PS2K_SCAN_CODE_SET && pendingData == 0)) && scanCodeCount == 0){
                scanCodeCount++;
                scanCodes[scanCodeCount] = PS2_GetScanCode();
            } else if (pendingCommand == PS2K_RESET || (pendingCommand == PS2K_SCAN_CODE_SET && pendingData == 0)){
                keyboardCallback(pendingCommand, scanCodes[1], PS2_GetScanCode());
                scanCodeCount = 3;
            } else if (pendingData != -1 && scanCodeCount == 0){
                PS2_Send((unsigned char)pendingData);
                scanCodeCount++;
            } else {
                if (pendingData != -1){
                     // Inform client of the command result
                    keyboardCallback(pendingCommand, (unsigned char)pendingData, PS2_GetScanCode());
                } else {
                    // Inform client of the command result
                    keyboardCallback(pendingCommand, PS2_GetScanCode(), 0);
                }
                scanCodeCount = 3;
            }
        }
    }
    
    // Do not allow it to pass the array size.  Also used to clear contents.
    if (scanCodeCount == 3){
        scanCodeCount = 0;
        scanCodes[0] = 0;
        scanCodes[1] = 0;
        scanCodes[2] = 0;
        pendingCommand = PS2K_NOP;
        // Can do next command if everything is cleared out.
        PS2K_ProcessQueue();
    }
}

void PS2K_Callback(PS2Keyboard_Callback_Function function){
    keyboardCallback = function;
}

void PS2K_Init(){
    PS2_Init();
    PS2_Callback(PS2P_Available_Int);
}
void PS2K_ResetKeyboard(){
    PS2K_AddToQueue(PS2K_RESET, -1);
    PS2K_ProcessQueue();
}
void PS2K_Echo(){
    PS2K_AddToQueue(PS2K_ECHO, -1);
    PS2K_ProcessQueue();
}
void PS2K_SetLED(unsigned char data){
    PS2K_AddToQueue(PS2K_LED, data);
    PS2K_ProcessQueue();
}
void PS2K_SetScanCodeSet(PS2K_SCAN_CODE_SETS set){
    PS2K_AddToQueue(PS2K_SCAN_CODE_SET, set);
    PS2K_ProcessQueue();
}
void PS2K_GetScanCodeSet(){
    PS2K_AddToQueue(PS2K_SCAN_CODE_SET, 0);
    PS2K_ProcessQueue();
}
void PS2K_EnableScanning(){
    PS2K_AddToQueue(PS2K_ENABLE_SCAN, -1);
    PS2K_ProcessQueue();
}
void PS2K_DisableScanning(){
    PS2K_AddToQueue(PS2K_DISABLE_SCAN, -1);
    PS2K_ProcessQueue();
}
void PS2K_SetToDefaults(){
    PS2K_AddToQueue(PS2K_DEFAULTS, -1);
    PS2K_ProcessQueue();
}
void PS2K_SetTypematicRateDelay(unsigned char repeatRate, PS2K_REPEAT_DELAY delay){
    unsigned char data = (repeatRate & 0x1f) + delay;
    PS2K_AddToQueue(PS2K_TYPE_RATE, data);
    PS2K_ProcessQueue();
}
void PS2K_ResendLastByte(){
    PS2K_AddToQueue(PS2K_RESEND, -1);
    PS2K_ProcessQueue();
}


